#include "../../headers/structsystem.h"
using namespace std;

double rampStep(double rtime, double time)
{
    return 1.0;
}

void showParticleMovement(const double arr[])
{
    for (int i = 0; i < 6; i++)
    {
        cout << arr[i] << ", ";
    }
    cout << endl;
}

void showParticleMovement(const StdArray6d &arr)
{
    for (int i = 0; i < 6; i++)
    {
        cout << arr[i] << ", ";
    }
    cout << endl;
}

void showParticleMovement(const StdArray6d* arr)
{
    for (int i = 0; i < 6; i++)
    {
        cout << (*arr)[i] << ", ";
    }
    cout << endl;
}


void showParticleForce(const vector<double> *v1)
{
    for (int i = 0; i < 6; i++)
    {
        cout << (*v1)[i] << ", ";
    }
    cout << endl;
}

void showParticleForce(const StdArray6d *v1)
{
    for (int i = 0; i < 6; i++)
    {
        cout << (*v1)[i] << ", ";
    }
    cout << endl;
}

void showParticleForce(const StdArray6d &v1)
{
    for (int i = 0; i < 6; i++)
    {
        cout << v1[i] << ", ";
    }
    cout << endl;
}

int main()
{
    // create system
    StructSystem system = StructSystem(1);
    system.setJobname("test");

    // solve parameters
    double endTime = 100.0;
    double h = 0.0001;
    double zeta = 0.2;
    double pa = 20.0;
    double pb = 20.0;
    double rtime = 50.0;

    // create particles
    Particle *p1 = new Particle(1, 0, 0);
    Particle *p2 = new Particle(2, 10, 0);
    Particle *p3 = new Particle(3, 10, 5);    // test addParticle
    cout << "\n----- Test addParticle -----" << endl;
    system.addParticle(p1);
    // system.info();
    vector <Particle *> ps;
    ps.push_back(p2);
    ps.push_back(p3);
    system.addParticles(ps);
    // system.info();

    // create material
    LinearElastic mat1 = LinearElastic(1);
    mat1.setE(2.0E5);
    mat1.setDensity(40.0/3.0);
    mat1.setNu(0.0);

    LinearElastic mat2 = LinearElastic(2);
    mat2.setE(1.0E5);
    mat2.setDensity(40.0/3.0);
    mat2.setNu(0.0);

    cout << "\n----- Test addElement -----" << endl;
    // create section
    CustomSectionParas paras = {.A=0.1, .Sy=0, .Sz=0, .SHy=0, .SHz=0,
                          .CGy=0, .CGz=0.0};
    CustomSection sect = CustomSection(1, paras);
    // create elements
    Link2D *e1 = new Link2D(1, p1, p2, &mat1, &sect);
    Link2D *e2 = new Link2D(2, p2, p3, &mat2, &sect);

    // system.addElement(e1);
    vector <StructElement *> elems;
    // system.info();
    elems.push_back(e1);
    elems.push_back(e2);
    system.addElement(e1);
    system.addElement(e2);
    // system.info();

    // constraints
    cout << "\n----- Test constraints -----" << endl;
    // vector<double> v1(6, 0);
    DOF c1 = {.key={true, true, true, true, true, true},
              .val={0, 0, 0, 0, 0, 0}};
    system.addConstraint(1, c1);
    system.addConstraint(3, c1);
    system.info();

    // vector<double> v2(6, 1);
    // system.changeConstraint(1, v2);
    // system.info();
    // system.deleteConstraint(3);
    // system.info();
    // system.clearConstraint();
    // system.info();

    // curve_value = rampStep(rtime, 0)
    StdArray6d f1 {0};
    StdArray6d f2 {0};
    StdArray6d f3 {0};
    f1[1] = -pa;
    f2[0] = pb;
    f2[1] = -pa;
    f3[0] = pb;
    system.addExternalForce(1, f1);
    system.addExternalForce(2, f2);
    system.addExternalForce(3, f3);
    system.solve(h, zeta, true);
    system.setInternalForce();

    int nStep = ceil(endTime / h);
    cout << nStep << endl;
    for (int i = 0; i < nStep; i++)
    {
        system.solve(h, zeta);
        // system.clearParticleForce();
        system.setExternalForce(1, f1);
        system.setExternalForce(2, f2);
        system.setExternalForce(3, f3);
        system.setInternalForce();
        // add external force
    }

    // show results
    cout << "---------- Particle deformation ----------" << endl;
    cout << "p1: ";
    showParticleMovement(p1->displace());
    cout << "p1: ";
    showParticleMovement(p2->displace());
    cout << "p1: ";
    showParticleMovement(p3->displace());

    cout << "---------- Particle force ----------" << endl;
    cout << "p1: ";
    const StdArray6d* r1 = p1->force();
    showParticleForce(r1);
    cout << "p1: ";
    showParticleForce(p2->force());
    cout << "p1: ";
    showParticleForce(p3->force());

    delete p1;
    delete p2;
    delete p3;
    delete e1;
    delete e2;
    return 0;
}